"""Contains pathfinding algorithms based on graph search"""

from __future__ import division

import heapq
from pyre.core import types
from pyre.ai import maps

class _Node(types.Coordinate):

    def __init__(self, pos, h_est):
        types.Coordinate.__init__(self, pos[0], pos[1])
        self.g_cost = float('inf')
        self.h_est = h_est
        self.prev = None

    @property
    def f_score(self):
        return self.g_cost + self.h_est

    def get_path(self):
        if self.prev:
            path = self.prev.get_path()
            path.append(self)
        else:
            path = []
        return path

    def __lt__(self, other):
        return self.f_score < other.f_score


class _NodeSet(object):

    def __init__(self, heuristic, start, goal):
        self.nodes = {}
        self.heuristic = heuristic
        self.goal = goal
        self[start].g_cost = 0

    def __getitem__(self, pos):
        if pos not in self.nodes:
            self.nodes[pos] = _Node(pos, self.heuristic(pos, self.goal))
        return self.nodes[pos]


def node_distance(start, goal):
    """
    node_distance(Coordinate, Coordinate): float
    Returns the Cartesian distance between two Coordinates
    """

    return start.distance_to(goal)


def zero_heuristic(start, goal):
    """
    zero_heuristic(Coordinate, Coordinate): float
    Always returns the value 0 as a heuristic.
    """

    return 0


def tiebreak_heuristic(heuristic, max_path_cost):
    """
    tiebreak_heuristic(func, float): func
    Creates a new tie-breaking heuristic from another heuristic by multiplying
    by a small factor based on the maximum expected cost of a path. As long as
    this estimate is never an under-estimate, the heuristic will still be
    admissible, but the heuristic will guide the search much better.
    """

    max_path_cost = 1 + 1 / max_path_cost
    return lambda start, goal: heuristic(start, goal) * max_path_cost


def create_search(heuristic, cost=node_distance, expand=maps.expand_adjacent):
    """
    create_search(func, func, func): func
    Creates a new graph search based on a heuristic and cost function, which
    take as input two Coordinate, and an expand function, which takes in a 
    Coordinate and a World, and should return all Coordinates reachable from
    the given node.
    """

    def graph_search(world, start, goal):
        """
        graph_search(World, Coordinate, Coordinate): list of Coordinate
        Attempts to find a path from the start to the goal on the given World
        using the heuristic, cost and expand functions given when the search
        was created.
        """

        assert world.inbounds(start)
        assert world.passable_at(goal)

        nodes = _NodeSet(heuristic, start, goal)

        closed = set()
        frontier = [nodes[start]]

        while frontier:
            node = heapq.heappop(frontier)
            if node in closed:
                continue
            closed.add(node)

            if node == goal:
                return node.get_path()

            for neighbor in expand(node, world):
                if neighbor not in closed:
                    neighbor = nodes[neighbor]
                    g_cost = node.g_cost + cost(node, neighbor)

                    if g_cost < neighbor.g_cost:
                        neighbor.g_cost = g_cost
                        neighbor.prev = node
                        heapq.heappush(frontier, neighbor)

    return graph_search


astar = create_search(node_distance)
greedy = create_search(node_distance, zero_heuristic)
